/*
 * @Author: tianchunyu
 * @Date: 2020-05-06 11:08:55
 * @LastEditTime: 2020-05-07 10:41:33
 * @LastEditors: Please set LastEditors
 * @Description: offer AGV wheel run control function and drive is synron
 * @FilePath: \X\app\xp\driver\wheel_s.c
 */
#include "wheel_s.h"
static u8 motor_if_enble = 0;

/*
该结构体是电机速度档位数组 

*/
static u16 speed_grade[SPEED_ABS_GRADE] = {0,100,200,300,400,500,600,700,800,900,1000,1100,1200};
static WHEEL_object agv_wheel={0};


/**
 * @description: Configuration of drives and handles,and set its some specific value
 * @param void  
 * @return: void
 */
void xp_wheel_config(void)
{
    
    synro_handler_A.config.direct = 0; //规定A电机的出厂方向为正   值就是默认的A为左轮
    synro_handler_B.config.direct = 1; //规定B电机的出厂向为负     B为右轮要改
    synro_handler_A.config.acc_time = 2000; //ms 加速时间
    synro_handler_A.config.dcc_time = 800; //ms 减速
    synro_handler_B.config.acc_time = 2000; //ms 
    synro_handler_B.config.dcc_time = 800; //
    /**/
    
}


/**
 * @description:     drive wheel initialization
 * @param   
    mode :  1：转矩运行(暂不支持)； 2：速度运行； 3：增量位置运行； 4:绝对位置运行
    port :  串口号
    speed ：波特率
 * @return: 
 *          0=success
 *          -1=fail
 */
int xp_wheel_init(u8 mode,u8 port,u32 speed)
{
    int sta=0;
    int dir =0;
    if(agv_wheel.init==1){
        return 0;
    }
    xp_wheel_config();
    sta = xp_synron_init(mode,port,speed);
    if(sta != SYNRON_SUCCESS)
    {
        LOG_SYNRON("wheel  motor init fail! xp_synron_init() return code=%d",sta);
        return -1;
    }
    dir = synron_read_hold(&synro_handler_A,PARAMETER_ROTA_DIRECTION);
    LOG_SYNRON("PARAMETER_ROTA_DIRECTION synro_handler_A dir is  %d",dir);
    if(dir == 0)
    {
        /*
        由于电机外接减速器，所以逻辑正好相反
        重新定义A  机旋转方向重启上电生效  规定A 电机也就是小车右电机顺时针为正 为了和小车的速度进行匹配
        */
        synron_set_default_rotation(&synro_handler_A,1);
        LOG_SYNRON("set A  motor default positive rotation ！！！！and synron motordrive need to be reboot");
          
    }
    agv_wheel.motor[0]=&synro_handler_A;
    agv_wheel.motor[1]=&synro_handler_B;
    agv_wheel.init = 1;
    motor_if_enble = 1;
    LOG_SYNRON("wheel initialize success  ");
    return 0;
}


int xp_wheel_motor_if_enble(SYNRON_TYPE if_enble)
{
    int ret1 = 0;
    int ret2 = 0;
    ret1 = synron_en(&synro_handler_A,if_enble);
    ret2 = synron_en(&synro_handler_B,if_enble);
    if((ret1 != SYNRON_SUCCESS)||(ret2 != SYNRON_SUCCESS ))
    {
        LOG_SYNRON(" xp_wheel_motor_if_enble fail!!! if_enble %d",if_enble);
        return SYNRON_COMMUN_ERR;
    }
    if(if_enble == syn_on)
    {
        motor_if_enble = 1;
				println(" motor_if_enble = 1;");
    }
    else if(if_enble == syn_off)
    {
        motor_if_enble = 0;
			  println(" motor_if_enble = 0;");
    }
    return SYNRON_SUCCESS;

}

int xp_wheel_get_enble_state()
{
    return motor_if_enble;
}



/**
 * @description: motor run 
 * @param {type} 
 * run:run state,1=启动，0=停止
    dir:run direction，0=正转，1=反转
 * @return: 
 *     0=success    
 *     -1=fail 
 * 本函数对于森创驱动器没多大作用，森创驱动器在速度模式下只要有速度就会转，如果停止直接让速度变成0，如果停止，直接调用
 * xp_wheel_move(u8,0) 即可
 */
int xp_motor_run(u8 run,u8 dir){
    

    return 0;
}


/*
 * @description: xp get the real robot speed 
 * @param {type} 
 * @return: 
 */
int xp_wheel_get_speed(float line_speed)
{
    float wheel_r;
    float motor=0;
    
        
    if(line_speed>SPEED_MAX_LIMIT)
    {
        LOG_SYNRON("speed is %f and its vaule exceed 0.3m/s");
        line_speed = SPEED_MAX_LIMIT;
    }
    if(line_speed<-SPEED_MAX_LIMIT)
    {
        LOG_SYNRON("speed is %f and its vaule exceed 0.3m/s");
        line_speed = -SPEED_MAX_LIMIT;
    }
    wheel_r=line_speed/(3.141593*WHEEL_DIAMETER);
    motor=wheel_r*60.00*WHEEL_MOTOR_REDUCE_RATIO;        //WHEEL_MOTOR_REDUCE_RATIO as Gear reduction ratio  motor unit prm
    return (int)motor;
}

/*
 * @description: xp get the real motor speed 
 * @param motorspeed motor rotation speed prm
 * @return: 
 *          robot wheel speed 
 */
float xp_wheel_get_motor_speed(s16 motorspeed)
{   
    double temf;
    temf = ((float)motorspeed)/(WHEEL_MOTOR_REDUCE_RATIO * 60.00);
    temf = temf *3.141593 *WHEEL_DIAMETER;
    return((float)temf);
}


/**
 * @description: xp_wheel_move 
 * @param  speed: robot move speed grade and see speed_grade array
 * @param  dir: dir >0 move ahead  dir <0 move back  dir = 0 move stop
 * @return: 
 */
int xp_wheel_move(u8 speed,s8 dir)
{
    int ret=0;
    s16 motor_speed = 0;
    if(speed >SPEED_ABS_GRADE)
    {
        LOG_SYNRON("xp_wheel_move input speed error value is %d and  max SPEED_ABS_GRADE %d",speed,SPEED_ABS_GRADE);
        speed = 1;
    }
    LOG_SYNRON("robot  motor speed is %d ",speed_grade[speed]);
    /*
    此函数让电机慢慢停下逻辑有问题是通过不断调整速度，暂先跳过后续修改
    synron_speedmode_motor_slow_stop(&synro_handler_A,100);  //让左电机慢慢停下
    synron_speedmode_motor_slow_stop(&synro_handler_B,100);  //让右电机慢慢停下
    */
    ret = xp_wheel_set_motors_speed(0,0);
    if(ret != SYNRON_SUCCESS)
    {
         LOG_SYNRON("xp_wheel_set_motors_speed fail");
         return SYNRON_NOR_ERR;
         
    }
    if(dir >0)
    {
         LOG_SYNRON("=====>xp_wheel_move motor speed %d  !",speed_grade[speed]);
         ret = synron_speedmode_setvalue(&synro_handler_A, speed_grade[speed]);  //motor_speed 以前后为正负
        if(ret != 0)
         {
             LOG_SYNRON("=====>synron_speedmode_setvalue A fail !");
             return -1;
         }
         ret = synron_speedmode_setvalue(&synro_handler_B, speed_grade[speed]);  //motor_speed 以前后为正负
        if(ret != 0)
         {
             LOG_SYNRON("=====>synron_speedmode_setvalue B fail !");
             return -1;
         }
         agv_wheel.dir=dir;
    }
    else if(dir <0)
    {
         LOG_SYNRON("=====>xp_wheel_move motor -speed %d  !",speed_grade[speed]);
         ret = synron_speedmode_setvalue(&synro_handler_A, -speed_grade[speed]);  //motor_speed 以前后为正负
        if(ret != 0)
         {
             LOG_SYNRON("=====>synron_speedmode_setvalue A fail !");
             return -1;
         }
         ret = synron_speedmode_setvalue(&synro_handler_B, -speed_grade[speed]);  //motor_speed 以前后为正负
        if(ret != 0)
         {
             LOG_SYNRON("=====>synron_speedmode_setvalue B fail !");
             return -1;
         }
         agv_wheel.dir=dir;
    }
    else //dir == 0
    {
        /*
        此函数让电机慢慢停下逻辑有问题是通过不断调整速度，暂先跳过后续修改
        synron_speedmode_motor_slow_stop(&synro_handler_A,100);  //让左电机慢慢停下
        synron_speedmode_motor_slow_stop(&synro_handler_B,100);  //让右电机慢慢停下
        */
        /*
         synron_speedmode_motor_slow_stop(&synro_handler_A,100);  //让左电机慢慢停下
         synron_speedmode_motor_slow_stop(&synro_handler_B,100);  //让右电机慢慢停下
         */
         xp_wheel_set_motors_speed(0,0);
         agv_wheel.dir = 0;
    } 
    return 0;
}

/**
 * @description: robot set  both wheels speed for motor A and motor B
 * @param wheela_speed  robot A wheel speed m/s
 * @param wheelb_speed  robot B wheel speed m/s
 * @return: 
 *      0 success
 *      1 fail
 */
int xp_wheel_set_wheel_speed(float wheela_speed,float wheelb_speed)
{
        
        s16 motora_speeed = xp_wheel_get_speed(wheela_speed);
        s16 motorb_speeed = xp_wheel_get_speed(wheelb_speed);
        LOG_SYNRON("xp_wheel_set_wheel_speed motora_speeed %d motorb_speeed %d",motora_speeed,motorb_speeed);
        int ret =0;
        ret = synron_speedmode_setvalue(&synro_handler_A,motora_speeed);
        if(ret != SYNRON_SUCCESS)
        {
            LOG_SYNRON("xp_wheel_set_wheel_speeed  A fail motora_speeed is %d!!!",motora_speeed);
            xp_io_write_pin(BOARD_ID,INPUT_PIN_A,HIGH_STATE);//通信有问题紧急输入制动信号
            return SYNRON_COMMUN_ERR;
          
        }
        ret = synron_speedmode_setvalue(&synro_handler_B,motorb_speeed);
        if(ret != SYNRON_SUCCESS)
        {
            LOG_SYNRON("xp_wheel_set_wheel_speeed  B fail motora_speeed is %d!!!",motorb_speeed);
            xp_io_write_pin(BOARD_ID,INPUT_PIN_B,HIGH_STATE);//通信有问题紧急输入制动信号
            return SYNRON_COMMUN_ERR;
        }
        return SYNRON_SUCCESS;
}

/**
 * @description: robot set  both motor speed for motor A and motor B
 * @param motora_speed  motor A rotation speed prm
 * @param motorb_speed  motor B rotation speed prm
 * @return: 
 *      0 success
 *      1 fail
 */
int xp_wheel_set_motors_speed(s16 motora_speed,s16 motorb_speed)
{
    int ret =0;
    ret = synron_speedmode_setvalue(&synro_handler_A,motora_speed);
    if(ret !=SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_wheel_set_motors_speeed  A fail motora_speeed is %d!!!",motora_speed);
        xp_io_write_pin(BOARD_ID,INPUT_PIN_A,HIGH_STATE);//通信有问题紧急输入制动信号
        return SYNRON_COMMUN_ERR;
    }
    ret = synron_speedmode_setvalue(&synro_handler_B,motorb_speed);
    if(ret != SYNRON_SUCCESS)
    {        
        xp_io_write_pin(BOARD_ID,INPUT_PIN_B,HIGH_STATE);//通信有问题紧急输入制动信号
        LOG_SYNRON("xp_wheel_set_motors_speeed  B fail motorb_speeed is %d!!!",motorb_speed);
        return SYNRON_COMMUN_ERR;
    }
    return SYNRON_SUCCESS;
}


/**
 * @description: wheel turn
 * @param dir:turn direction left<0,middle=0,right>0
 * @return: 
 *      0 success
 *      1 fail
 */

int xp_wheel_turn(WHEEL_turn dir)
{
    s16 cur_rspeeda = synron_get_speedvalue(&synro_handler_A);  //unit prm
    s16 cur_rspeedb = synron_get_speedvalue(&synro_handler_B);
    s16 cur_averageab = (cur_rspeeda + cur_rspeedb)/2;
    /*
    synron_speedmode_motor_slow_stop(&synro_handler_A,100);  //让左电机慢慢停下
    synron_speedmode_motor_slow_stop(&synro_handler_B,100);  //让右电机慢慢停下
    */
    xp_wheel_set_motors_speed(0,0);
    s16 bias_speed = (s16)(cur_rspeedb*WHEEL_TURN_FAC);   
    u16 wait_time =20000;
    /*
    小车前进方向和后退方向的左转和右转方向不一样
    */
    if(agv_wheel.dir>=0) //cur_averageab>0
    {
        switch (dir)
        {
        case wheel_left:
            xp_wheel_set_motors_speed(cur_averageab-bias_speed,cur_averageab+bias_speed);
            LOG_SYNRON("cur_averageab %d bias_speed %d",cur_averageab,bias_speed);
            break;
        case wheel_right:
            xp_wheel_set_motors_speed(cur_averageab+bias_speed,cur_averageab-bias_speed);
            LOG_SYNRON("cur_averageab %d bias_speed %d",cur_averageab,bias_speed);
            break;
        case wheel_middle:
            xp_wheel_set_motors_speed(cur_averageab,cur_averageab);
            break;
        default:
            break;
        }
    
    }
    if(agv_wheel.dir<0) //cur_averageab<0
    {
        switch (dir)
        {
        case wheel_left:
            xp_wheel_set_motors_speed(cur_averageab+bias_speed,cur_averageab-bias_speed);
            LOG_SYNRON("cur_averageab %d bias_speed %d",cur_averageab,bias_speed);
            break;
        case wheel_right:
            xp_wheel_set_motors_speed(cur_averageab-bias_speed,cur_averageab+bias_speed);
            LOG_SYNRON("cur_averageab %d bias_speed %d",cur_averageab,bias_speed);
            break;
        case wheel_middle:
            xp_wheel_set_motors_speed(cur_averageab,cur_averageab);
            break;
        default:
            break;
        }
    
    }
    aos_msleep(wait_time);
    xp_wheel_set_motors_speed(cur_averageab,cur_averageab);
    return SYNRON_SUCCESS;
}

/**
 * @description: Set the acceleration time of the two motors 
 * @param time_ms unit ms
 * @return: 
 *         0 success
 *         -1 fail
 */
int xp_wheel_set_motors_acc_timems(u16 time_ms)
{
    int ret = 0;
    if(time_ms > 5000 ||time_ms< 0)
    {
        LOG_SYNRON("times_ms is invalid  and its value is %d  \n",time_ms);        
        return SYNRON_NOR_ERR;
    }        
    ret = synron_write_register(&synro_handler_A,PARAMETER_TIME_ACCELERATE,time_ms);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_wheel_set_motors_acc_timems A is %d\n",time_ms);  
        xp_io_write_pin(BOARD_ID,INPUT_PIN_A,HIGH_STATE);//通信有问题紧急输入制动信号
        return SYNRON_COMMUN_ERR;           
    }
    ret = synron_write_register(&synro_handler_B,PARAMETER_TIME_ACCELERATE,time_ms);
    if(ret != SYNRON_SUCCESS)
    {
        xp_io_write_pin(BOARD_ID,INPUT_PIN_B,HIGH_STATE);//通信有问题紧急输入制动信号       
        LOG_SYNRON("xp_wheel_set_motors_acc_timems B is %d\n",time_ms);        
        return SYNRON_COMMUN_ERR;           
    }
    return SYNRON_SUCCESS;
}



/**
 * @description: set the deceleration time of the two motors
 * @param time_ms unit ms
 * @return: 
 *         0 success
 *         -1 fail
 */
int xp_wheel_set_motors_dcc_timems(u16 time_ms)
{
   int ret = 0;
    if(time_ms > 5000 ||time_ms< 0)
    {
        LOG_SYNRON("times_ms is invalid  and its value is %d  \n",time_ms);        
        return SYNRON_NOR_ERR;
    }        
    ret = synron_write_register(&synro_handler_A,PARAMETER_TIME_DECELERATION,time_ms);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_wheel_set_motors_acc_timems A is %d\n",time_ms);      
        xp_io_write_pin(BOARD_ID,INPUT_PIN_A,HIGH_STATE);//通信有问题紧急输入制动信号
        return SYNRON_COMMUN_ERR;           
    }
    ret = synron_write_register(&synro_handler_B,PARAMETER_TIME_DECELERATION,time_ms);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_wheel_set_motors_acc_timems B is %d\n",time_ms);
        xp_io_write_pin(BOARD_ID,INPUT_PIN_B ,HIGH_STATE);//通信有问题紧急输入制动信号
        return SYNRON_COMMUN_ERR;           
    }
    return SYNRON_SUCCESS;
}

/*
* @description: set the deceleration time and acceleration time of the two motors
* @param time_ms unit ms
* @return: 
*         0 success
*         -1 fail
*
*/
int xp_wheel_set_motors_acc_dcc_timems(u16 time_ms)
{
    int ret =0;
    if(time_ms > 5000 ||time_ms< 0)
    {
        LOG_SYNRON("times_ms is invalid  and its value is %d  \n",time_ms);        
        return SYNRON_NOR_ERR;
    }        
    ret = xp_wheel_set_motors_acc_timems(time_ms);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_wheel_set_motors_acc_timems B is %d\n",time_ms);        
        return SYNRON_COMMUN_ERR;              
    }
    ret = xp_wheel_set_motors_dcc_timems(time_ms);
    if(ret != SYNRON_SUCCESS)
    {
        LOG_SYNRON("xp_wheel_set_motors_acc_timems B is %d\n",time_ms);        
        return SYNRON_COMMUN_ERR;              
    }
    return SYNRON_SUCCESS;
}


/**
 * @description: don't move and  rotate to a certain angle
 * @param turn direction left<0,middle=0,right>0
 * @param angle certain angle
 * @return: 
 */
int xp_wheel_round(WHEEL_turn turn,float angle)
{
    s16 rotate_speed = WHEEL_SITU_ROTATE_SPEED;
    if(agv_wheel.dir!=0){
        LOG_SYNRON("error:please stop wheel motor first!");
        return -1;
    }
    if(turn == wheel_left)
    {
        xp_wheel_set_motors_speed(-rotate_speed,rotate_speed);
    }
    else if(turn == wheel_right)
    {
        xp_wheel_set_motors_speed(rotate_speed,-rotate_speed);
    }
    else
    {   
        xp_wheel_set_motors_speed(0,0);
    }
     //get imu angle
    // while(abs(imu[2]-imu[1])<(angle-5)){
    //     aos_msleep(100);
    // }
    //synron_speedmode_motor_slow_stop(&synro_handler_A,100);  //让左电机慢慢停下
    //synron_speedmode_motor_slow_stop(&synro_handler_B,100);  //让右电机慢慢停下
    // xp_pans_slow_stop(&wheel_handler[0],pans_on);
    // xp_pans_slow_stop(&wheel_handler[1],pans_on);
    return 0;
}





/**
 * @description: read encode motorA and motorB
 * @param {type} 
 * @return: 
 */
void xp_wheel_read_encode(void)
{
    s32 encodea = 0;
    s32 encodeb =0;
    encodea = synron_get_abs_pulsevalue(&synro_handler_A);
    encodeb = synron_get_abs_pulsevalue(&synro_handler_B);
    LOG_SYNRON("encodea: %d and encodeb: %d",encodea,encodeb);

}

/**
 * @description: set encode value in a specific value
 * @param {type} 
 * @return: 0  success
 *          -1 fail
 *          
 */

int xp_wheel_set_encode_specifice_value(s32 value)
{
   int ret1 = 0;
   int ret2 = 0;
   ret1 = synron_position_re_encode_setvalue(&synro_handler_A,value);
   ret2 = synron_position_re_encode_setvalue(&synro_handler_B,value);
   if(ret1 != 0 || ret2 != 0)
   {
     return -1;
   }
   return 0;
}
/**
 * @description: get left wheel encode
 * @param {type} 
 * @return: 0  success
 *          -1 fail
 *          
 */

int xp_wheel_get_left_encode()
{
   return  synron_get_abs_pulsevalue(&synro_handler_A);
}

/**
 * @description: get right wheel encode
 * @param {type} 
 * @return: 0  success
 *          -1 fail
 *          
 */

int xp_wheel_get_right_encode()
{
   return  synron_get_abs_pulsevalue(&synro_handler_B);
}



/*
get wheel move dir
return:dir
*/
int xp_wheel_get_dir(void)
{
    s16 average_speed = 0;
    s16 motor_a = synron_get_speedvalue(&synro_handler_A);
    s16 motor_b = synron_get_speedvalue(&synro_handler_B);
    if(motor_a == 0 && motor_b == 0)
    {
        agv_wheel.dir = 0;
        return agv_wheel.dir ;
    }
    average_speed = (motor_a+motor_b)/2;
    if(average_speed> 0)
    {
        agv_wheel.dir = 1;
        return agv_wheel.dir ;
    }
    else 
    {
        agv_wheel.dir = -1;
        return agv_wheel.dir ;
    }
}





/********************************wheel motor test*************************************/
/**
 * @description: xp_wheel_debug
 * @param type:debug type
 * @param fun:function
 * @param param:type: 
 *              fun function 
 *              param     
 * @return: 
 */
int xp_wheel_debug(char *type,char *fun,char *param)
{
    char param1[10] ={0};
    char param2[10] ={0};
    u8 speedu = 0;
    s8 dir = 0;
    s16 speedsl = 0;
    s16 speedsr = 0;
    float speedfl = 0;
    float speedfr = 0;
    float angle = 0;
    if(type==NULL)
    {
        LOG_SYNRON("type  is null");
        return 0;
    }
    if(strcmp(type,"wheel")==0)
    {
        if(strcmp(fun,"init") == 0)
        {
            LOG_SYNRON("xp_wheel_init");
            xp_wheel_init(2,1,115200);
            return 1;
        }
        if(agv_wheel.init == 1)
        {
            if(strcmp(fun,"move")==0)
            {
                LOG_SYNRON("param is %s \n",param);
                memset(param1,0,sizeof(param1));
                memset(param2,0,sizeof(param2));
                memcpy(param1,param,strlen(param)-1);
                memcpy(param2,param+strlen(param)-1,1);
                LOG_SYNRON("param1 %s and param2 %s",param1,param2);
                speedu = strtoul(param1, NULL, 10);
                dir =  strtoul(param2, NULL, 10);
                LOG_SYNRON("speedu %d and dir %d ",speedu,dir);
                /*
                 参数不够了只能这样凑了
                */
                if(dir >1)
                {
                    dir = -1;
                }
                xp_wheel_move(speedu,dir);
                
            }
            else if(strcmp(fun,"rencode") == 0)
            {
                xp_wheel_read_encode();
            }
            else if(strcmp(fun,"clearencode") == 0)
            {
                speedu = xp_wheel_set_encode_specifice_value(0);
                if(speedu != 0)
                {
                   LOG_SYNRON("xp_wheel_set_encode_specifice_value set value 0 fail"); 
                }
                else
                {
                   LOG_SYNRON("xp_wheel_set_encode_specifice_value set value 0 success"); 
                }
            }
            else if(strcmp(fun,"round") == 0)
            {
                dir =  strtoul(param, NULL, 10);
                LOG_SYNRON("round dir is %d",dir);
                xp_wheel_round(dir,angle);
            }
            else if(strcmp(fun,"turn") == 0)
            {
                 dir =  strtoul(param, NULL, 10);
                LOG_SYNRON("turn dir is %d",dir);
                xp_wheel_turn(dir);
            }
            else if(strcmp(fun,"acctime") == 0)
            {
                speedsl =  strtoul(param, NULL, 10);
                LOG_SYNRON("acctime is %d ms",speedsl);
                xp_wheel_set_motors_acc_timems(speedsl);
            }
            else if(strcmp(fun,"dcctime") ==0)
            {
                speedsl =  strtoul(param, NULL, 10);
                LOG_SYNRON("dcctime is %d ms",speedsl);
                xp_wheel_set_motors_dcc_timems(speedsl);
            }
            else if(strcmp(fun,"accdcc") ==0)
            {
                speedsl =  strtoul(param, NULL, 10);
                LOG_SYNRON("acctime dcctime is %d ms",speedsl);
                xp_wheel_set_motors_acc_dcc_timems(speedsl);
            }
            else if(strcmp(fun,"test_st") == 0)
            {
                 speedsl =  strtoul(param, NULL, 10);
                 LOG_SYNRON("speed  is  %d and start stable test ！！！！",speedsl);
                 while(1)
                 {
                    xp_wheel_set_motors_speed(speedsl,speedsl);
                    aos_msleep(5000);
                    xp_wheel_set_motors_speed(-speedsl,-speedsl);
                    aos_msleep(5000);
                 }
            }
            return 1;
        }
        else
        {
            LOG_SYNRON("xp_wheel_init need to run");
            return 0;
        }
    }
    else if(strcmp(type,"wheelms") == 0)
    {
         speedsl = strtoul(fun, NULL, 10);
         speedsr = strtoul(param, NULL, 10);
         LOG_SYNRON("xp_wheel_set_motors_speeed speedsl %d speedsr %d",speedsl,speedsr);
         xp_wheel_set_motors_speed(speedsl,speedsr);
         return 1;
    }
    else if(strcmp(type,"wheelrs") == 0)
    {
         speedfl = atof(fun);
         speedfr = atof(param);
         if(speedfl > 0.5 || speedfl <-0.5)
         {
             LOG_SYNRON("xp_wheel_set_motors_speeed speedsl %f speedsr %f error and don't exceed 0.5m/s",speedfl,speedfr);
         }
         LOG_SYNRON("xp_wheel_set_motors_speeed speedsl %f speedsr %f",speedfl,speedfr);
         xp_wheel_set_wheel_speed(speedfl,speedfr);
         return 1;
         
    }
    
    else
    {
        return 0;
    }
    
}






